#include "lqr.h"

LQR::LQR(){
    std::vector<double> diag_vec = {1.0, 1.0, 1.0}; //[x,y,theta]
    Q_ = Eigen::Matrix3d::Zero();
    for (size_t i = 0; i < diag_vec.size(); ++i)
      Q_(i, i) = diag_vec[i];
    
    diag_vec = {1.0, 1.0};   //[v, w]
    R_ = Eigen::Matrix2d::Zero();
    for (size_t i = 0; i < diag_vec.size(); ++i)
      R_(i, i) = diag_vec[i];
    
    dt_ = 1.0 / baseConfig_.controller_frequency;
    max_iter_ = 100;
    eps_iter_ = 1e-4;
}


LQR::~LQR(){

}

// 计算局部目标点的速度
// mc::status LQR::computeTwist(geometry_msgs::PoseStamped goal, geometry_msgs::Twist curr_vel, geometry_msgs::Twist& cmd_vel){
mc::status LQR::computeTwist(mc::TrajectoryPoint2D goal, geometry_msgs::Twist curr_vel,
    geometry_msgs::Twist& cmd_vel){
    mc::status status;

    // if(isPositionReached(robot_posestamp_, goal)){
    //     if(!isAngleReached(robot_posestamp_, goal)){
    //         rotateToTGoal(robot_posestamp_, goal, cmd_vel);
    //     }
    // } 
    // else{
    //     Eigen::Vector3d curr_pose(0.0, 0.0, 0.0);   
    //     Eigen::Vector3d goal_pose(goal.pose.position.x, 
    //         goal.pose.position.y, 
    //         tf2::getYaw(goal.pose.orientation));

    //     double vt = std::hypot(curr_vel.linear.x, curr_vel.linear.y);
    //     double wt = curr_vel.angular.z;
    //     // Eigen::Vector2d u_r(vt, vt * kappa); 
    //     Eigen::Vector2d u_r(vt, vt * 0.1); 
    //     Eigen::Vector2d u = LQR_Slove(curr_pose, goal_pose, u_r);

    //     cmd_vel.linear.x = u[0];
    //     cmd_vel.angular.z = u[1];
    // }

    return status;
}


// LQR 求解计算
// https://mr-winter.blog.csdn.net/article/details/137479248
// https://blog.csdn.net/qq_44339029/article/details/137593464
Eigen::Vector2d LQR::LQR_Slove(Eigen::Vector3d s, Eigen::Vector3d s_d, Eigen::Vector2d u_r){
    Eigen::Vector2d u;
    Eigen::Vector3d e(s - s_d);
    e[2] = utils::math::normalizeAngle(e[2]);

    // state equation on error
    Eigen::Matrix3d A = Eigen::Matrix3d::Identity();
    A(0, 2) = -u_r[0] * sin(s_d[2]) * dt_;
    A(1, 2) = u_r[0] * cos(s_d[2]) * dt_;

    Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3, 2);
    B(0, 0) = cos(s_d[2]) * dt_;
    B(1, 0) = sin(s_d[2]) * dt_;
    B(2, 1) = dt_;

    // discrete iteration Ricatti equation
    Eigen::Matrix3d P, P_;
    P = Q_;
    for (int i = 0; i < max_iter_; ++i){
        Eigen::Matrix2d temp = R_ + B.transpose() * P * B;
        P_ = Q_ + A.transpose() * P * A - A.transpose() * P * B * temp.inverse() * B.transpose() * P * A;
        if ((P - P_).array().abs().maxCoeff() < eps_iter_)
        break;
        P = P_;
    }

    // feedback
    Eigen::MatrixXd K = -(R_ + B.transpose() * P_ * B).inverse() * B.transpose() * P_ * A;
    u = u_r + K * e;
    return u;
}